#include <stddef.h>
#include <string.h>
#include <sys/types.h>
#include "memory.h"
#include "sysdef.h"
#include "os.h"

static OS_MUTEX heap_mutex;

void * __real_malloc(size_t size);
void * __real_calloc(size_t nitems, size_t size);
void * __real_realloc(void *ptr, size_t size);
void __real_free(void* ptr);

void* __wrap_malloc(size_t size)
{
    OS_ERR err;
    void *ptr = NULL;
    OSMutexPend(&heap_mutex, OS_WAIT_FOREVER, OS_OPT_PEND_NON_BLOCKING, NULL, &err);
    ptr =  __real_malloc(size);
    OSMutexPost(&heap_mutex, OS_OPT_POST_NONE, &err);

    return ptr;
}

void* __wrap_calloc(size_t nitems, size_t size)
{
    OS_ERR err;
    void *ptr = NULL;
    OSMutexPend(&heap_mutex, OS_WAIT_FOREVER, OS_OPT_PEND_NON_BLOCKING, NULL, &err);
    ptr =  __real_calloc(nitems, size);
    OSMutexPost(&heap_mutex, OS_OPT_POST_NONE, &err);

    return ptr;
}

void* __wrap_realloc(void *ptr, size_t size)
{
    OS_ERR err;
    void *ptr2 = NULL;
    OSMutexPend(&heap_mutex, OS_WAIT_FOREVER, OS_OPT_PEND_NON_BLOCKING, NULL, &err);
    ptr2 =  __real_realloc(ptr, size);
    OSMutexPost(&heap_mutex, OS_OPT_POST_NONE, &err);

    return ptr2;
}

void __wrap_free(void* ptr)
{
    OS_ERR err;
    OSMutexPend(&heap_mutex, OS_WAIT_FOREVER, OS_OPT_PEND_NON_BLOCKING, NULL, &err);
    __real_free(ptr);
    OSMutexPost(&heap_mutex, OS_OPT_POST_NONE, &err);
}

caddr_t _sbrk(int increment)
{
    extern char end[];
    extern char __stack_bottom[];

    static char *s_pHeapEnd = NULL;

    if (!s_pHeapEnd)
        s_pHeapEnd = end;

    if (s_pHeapEnd + increment > __stack_bottom)
        return (caddr_t)-1;

    char *pOldHeapEnd = s_pHeapEnd;
    s_pHeapEnd += increment;
    return (caddr_t)pOldHeapEnd;
}

void memory_init(void)
{
    OS_ERR err;
    OSMutexCreate(&heap_mutex, "heap_mutex", &err);
}
